package futbol.tacticas.concursantes.capello_team;

import futbol.General;
import java.awt.*;
import java.util.Vector;
import futbol.tacticas.*;
import futbol.tacticas.concursantes.Tactica;
import java.lang.Math;
/**
 *
 * CAPELLO_TEAM
 */
public class capello_team implements Tactica{
    

	
    private int nValores[][]={
            {-150,0,6}, 
            {-80,0,7},  
            {-70,45,7}, 
            {-70,-45,7}, 
            {-5,-30,6}, 
            {-20,0,6}, 
            {-5,30,6},  
            {60,50,6},
            {60,-50,6}, 
            {60,15,7},  
            {60,-15,7} 
    };

    
    private int tiempo=0;
    private int jugmio=-1; 
    private double ba[]=new double[2];
    private double bac[]=new double[2];
    private double anguloba=0.0;
    private double xAccesible=-99999;
    private double yAccesible=-99999;
    private double bolasup[]=new double[2]; 
    private double distRecAnt=0;
    private boolean jugadorCercaBola=true;
    private int tiempoBolaQuieta=0;
    
    
    public Color getColor1() {
        return Color.red;
    }

    public Color getColor2() {
        return Color.white;
    }

    public Color getColorNumero() {
        return Color.black;
    }

    public String getNombre() {
        return "Capello Team";
    }

    public int getXInicial(int n) {
        return nValores[n][0];
    }

    public int getYInicial(int n) {
        return nValores[n][1];
    }

    public int getFuerza(int n) {
        return nValores[n][2];
    }

    public Vector<Comando> getComandos(SituacionJugadores sj) {
        Vector<Comando> com=new Vector<Comando>() ;       
        Point jugadores[]=sj.getMisJugadores();
        Point contrarios[]=sj.getContrario();
        Point bola=sj.getBola();       
        
        if(this.tiempo==0)	
        {
        	 for (int n=0;n<11;n++)
        	 {
        		 com.add(new ComandoIrA(n,nValores[n][0], nValores[n][1]));  
        	 }
             this.ba[0]=0;
             this.ba[1]=0;
             this.bac[0]=0;
             this.bac[1]=0;
             this.bolasup[0]=-99999;
             this.bolasup[1]=-99999;

        }
        
        com=this.mover(com,sj);
        
        this.ba[0]=this.bac[0];
        this.ba[1]=this.bac[1];
        this.bac[0]=bola.x;
        this.bac[1]=bola.y;
        
             
       
        double distanciaRecorrida=this.getDistancia (this.ba[0], this.ba[1],this.bac[0], this.bac[1]);
        double anguloBola=this.getAngulo(this.ba[0], this.ba[1],this.bac[0], this.bac[1]);
       
        if ( (Math.abs(distanciaRecorrida - this.distRecAnt)>=2 || Math.abs(anguloBola - this.anguloba)>0.1) && distanciaRecorrida<23 && this.distRecAnt<16 )
        {
        		this.distRecAnt=this.getDistancia (this.ba[0], this.ba[1],this.bac[0], this.bac[1]);
        		this.anguloba=this.getAngulo(this.ba[0], this.ba[1],this.bac[0], this.bac[1]);
        		
        		this.deducirPuntoFinalBola();
        		this.deducirJugadorMejorPosicionado(sj);        	
        }
        this.distRecAnt=this.getDistancia (this.ba[0], this.ba[1],this.bac[0], this.bac[1]);
		this.anguloba=this.getAngulo(this.ba[0], this.ba[1],this.bac[0], this.bac[1]);
        
       
        
        
        if (this.jugmio==-1)
        {
        	this.jugmio=sj.getMasCercanoDeBola();
        }
        if(this.xAccesible==-99999)
        {
        	this.xAccesible=bola.x;
        }
        if(this.yAccesible==-99999)
        {
        	this.yAccesible=bola.y;
        }
    
        
        if (this.distRecAnt<=15)
        {
        	com.add(new ComandoIrA(this.jugmio,this.xAccesible, this.yAccesible));
        	com.add(new ComandoIrA(sj.getMasCercanoDeBola(),bola.x, bola.y)); 

        }        
        else
        {
        	com.add(new ComandoIrA(this.jugmio,xAccesible, yAccesible));	
        }
        
        if (this.distRecAnt<12 && !sj.estoyMasCercaDeBola() )
        {
        	int otroJug=this.buscarOtroJugadorPorDetras(sj);
        	if (otroJug!=-1)
        	{
        		com.add(new ComandoIrA(otroJug,bola.x, bola.y));
        	}
        	   
        }
        
 
              
   
    
        com=this.cancerbero(com,sj);


       
        
        for (int n=0 ; n<11 ; n++)
        { 
          
        		com=ponComando(com,sj,n);
        	
        }
        

        this.tiempo++;
        
        
     
        if (this.ba[0]==this.bac[0] && this.ba[1]== this.bac[1])
        {
        	this.tiempoBolaQuieta++;
        }
        else
        {
        	this.tiempoBolaQuieta=0;
        }
        
        if (this.tiempoBolaQuieta>60)
        {
        	com.add(new ComandoIrA(sj.getMasCercanoDeBola(),bola.x, bola.y));   
        }
        
        
        
        
        
        
        int contrarioMasCerca=sj.getContrarioMasCerca(bola.x, bola.y);
        int miJugadorMasCerca=sj.getMasCercanoDeBola();
        if (this.getDistancia(contrarios[contrarioMasCerca].x, contrarios[contrarioMasCerca].y, bola.x, bola.y)< 15 ||this.getDistancia(jugadores[miJugadorMasCerca].x, jugadores[miJugadorMasCerca].y, bola.x, bola.y)< 15)
        {
        	this.jugadorCercaBola=true; 	
        }
        else
        {
        	this.jugadorCercaBola=false;
        }
      return com;
    }
    
    
    
    
    private  Vector<Comando> cancerbero(Vector<Comando> com, SituacionJugadores sj)
    { 
    	Point misJugadores[]=sj.getMisJugadores();
    	Point contrarios[]=sj.getContrario();
    	Point bola=sj.getBola();
    	
        double y=bola.y;
        double x=-150;
        
        double distanciaPorteroBalon=this.getDistancia(misJugadores[0].x, misJugadores[0].y, bola.x, bola.y);
    	int contrarioMasCercanoBalon=sj.getContrarioMasCerca(bola.x, bola.y);
    	double distanciaContrarioBalon=this.getDistancia(contrarios[contrarioMasCercanoBalon].x, contrarios[contrarioMasCercanoBalon].y, bola.x, bola.y);
        
        if ( (bola.x<-140 && bola.y<60 && bola.y>-60 && distanciaPorteroBalon<distanciaContrarioBalon && sj.getMasCercanoDeBola()==0 && this.distRecAnt<16) || (distanciaPorteroBalon<13 && distanciaContrarioBalon>13 && this.distRecAnt<16))
        {        	
        	y=bola.y;
        	x=bola.x;
	
        }
       
        else
        {
        	if ( this.bac[0]!=this.ba[0] ) 
        	{
	        	x=-150;
	        	y= ( ( (this.bac[1] - this.ba[1])/(this.bac[0] - this.ba[0]) ) * (-150-this.ba[0]) ) + this.ba[1];
	        	if (bola.getX()>-125)
	            {
	    	        if (y<-20) 
	    	        {   
	    	            y=-20;
	    	        }
	    	            
	    	        if (y>20) 
	    	        {   
	    	            y=20;
	    	        }
	            }
        	}
        }
        
       
        if (sj.estoyMasCercaDeBola() && sj.getMasCercanoDeBola()==0)
        {
        	if ( cancerberoaccion(sj) )
        	{
        		x=bola.x;
        		y=bola.y;
        	}
        	else 
        	{
        		int otroJugador=this.buscarOtroJugador(sj);
        		 com.add( new ComandoIrA(otroJugador,bola.x,bola.y) );
        	}
        }
        
        if ( Math.abs( misJugadores[0].x) <140 ||Math.abs( misJugadores[0].y) >45 ) 
        																			
        {
        	if (distanciaPorteroBalon<30 && (sj.getMasCercanoDeBola()==0 || (distanciaPorteroBalon+5)<this.getDistancia(misJugadores[sj.getMasCercanoDeBola()].x, misJugadores[sj.getMasCercanoDeBola()].y, bola.x, bola.y)))
        	{
        		x=bola.x;
        		y=bola.y;
        	}
        }
        
        
        com.add( new ComandoIrA(0,x,y) );

    
    return com;
    }
    
    private  boolean cancerberoaccion (SituacionJugadores sj)
    {
    	boolean sale=false;
    	Point bola=sj.getBola();
    	Point jugadores[]=sj.getMisJugadores();
    	Point contrarios[]=sj.getContrario();
    	int contrarioMasCercaBalon=sj.getContrarioMasCerca(bola.x, bola.y);
    	double distanciaContrarioBalon=this.getDistancia(contrarios[contrarioMasCercaBalon].x, contrarios[contrarioMasCercaBalon].y, bola.x, bola.y);
    	double distanciaPorteroBalon=this.getDistancia(jugadores[0].x, jugadores[0].x, bola.x, bola.y);
	
    	
    
    if (this.bac[0]<-110)
    {
    	if (this.bac[0]<this.ba[0])
    	{
    		if ( Math.abs( ( ( (this.bac[1] - this.ba[1])/(this.bac[0] - this.ba[0]) ) * (-155-this.ba[0]) ) + this.ba[1] )<45 ) 
    		{
    			if (this.distRecAnt<6)
    			{
    				sale=true;
    			}
    		}
    		else
    		{
    			if (this.distRecAnt <10 && distanciaPorteroBalon<distanciaContrarioBalon )
    			{
    				sale=true;
    			}
    		}
    	}
    	
    	else
    	{
    		if (this.distRecAnt <8 && (distanciaPorteroBalon+7)<distanciaContrarioBalon )
			{
				sale=true;
			}
    		
    	}
    }
    else
    {
    	
	    if (this.distRecAnt <15 && (distanciaPorteroBalon+20)<distanciaContrarioBalon )
		{
			sale=true;
		}		
    			
    }
  
    
    	
    	
    	
    	
    return sale;
    }
    
   
    
    
    private  Vector<Comando> ponComando(Vector<Comando> com, SituacionJugadores sj,int n)
    {
    	Point misJugadores[]=sj.getMisJugadores();
    	Point contrarios[]=sj.getContrario();
    	Point bola=sj.getBola();
    	
    	
    	
    	int marcadorDel1=sj.getContrarioMasCerca(1);
    	int marcadorDel2=sj.getContrarioMasCerca(2);
    	int marcadorDel3=sj.getContrarioMasCerca(3);
    	int marcadorDel4=sj.getContrarioMasCerca(4);
    	int marcadorDel5=sj.getContrarioMasCerca(5);
    	int marcadorDel6=sj.getContrarioMasCerca(6);
    	double distanciaAl1=this.getDistancia(misJugadores[1].x, misJugadores[1].y, contrarios[marcadorDel1].x, contrarios[marcadorDel1].y);
    	double distanciaAl2=this.getDistancia(misJugadores[2].x, misJugadores[2].y, contrarios[marcadorDel2].x, contrarios[marcadorDel2].y);
    	double distanciaAl3=this.getDistancia(misJugadores[3].x, misJugadores[3].y, contrarios[marcadorDel3].x, contrarios[marcadorDel3].y); 
    	
    	double distanciaAl4=this.getDistancia(misJugadores[4].x, misJugadores[4].y, contrarios[marcadorDel4].x, contrarios[marcadorDel4].y);
    	double distanciaAl5=this.getDistancia(misJugadores[5].x, misJugadores[5].y, contrarios[marcadorDel5].x, contrarios[marcadorDel5].y);
    	double distanciaAl6=this.getDistancia(misJugadores[6].x, misJugadores[6].y, contrarios[marcadorDel6].x, contrarios[marcadorDel6].y); 	
    	
    	 	
    	
    	int marcadorDel8=sj.getContrarioMasCerca(8);
    	int marcadorDel9=sj.getContrarioMasCerca(9);
    	int marcadorDel10=sj.getContrarioMasCerca(10);
    	int marcadorDel7=sj.getContrarioMasCerca(7);
    	double distanciaAl9=this.getDistancia(misJugadores[9].x, misJugadores[9].y, contrarios[marcadorDel9].x, contrarios[marcadorDel9].y);
    	double distanciaAl10=this.getDistancia(misJugadores[10].x, misJugadores[10].y, contrarios[marcadorDel10].x, contrarios[marcadorDel10].y);
    	double distanciaAl7=this.getDistancia(misJugadores[7].x, misJugadores[7].y, contrarios[marcadorDel7].x, contrarios[marcadorDel7].y);
    	double distanciaAl8=this.getDistancia(misJugadores[8].x, misJugadores[8].y, contrarios[marcadorDel8].x, contrarios[marcadorDel8].y);
    	
    	int avanzarBolaX=0;
    	int avanzarBolaY=0;
    	if (misJugadores[n].y<5 && misJugadores[n].y>-5)
    	{
    		avanzarBolaX=10;
    		avanzarBolaY=0;
    	}
    	else
    	{
    		if (misJugadores[n].y<-5)
    		{
    			avanzarBolaX=6;
    			avanzarBolaY=8;    			
    		}
    		else
    		{
    			avanzarBolaX=6;
    			avanzarBolaY=-8; 
    		}
    	}
    	
    	
    	
    	int contrarioMasCercaBola=sj.getContrarioMasCerca(misJugadores[n].x + avanzarBolaX, misJugadores[n].y + avanzarBolaY);    	
    	double distanciaBolaAlContrario=this.getDistancia(misJugadores[n].x + avanzarBolaX, misJugadores[n].y + avanzarBolaY,contrarios[contrarioMasCercaBola].x, contrarios[contrarioMasCercaBola].y);
    	if (distanciaBolaAlContrario>24 && n>3 && bola.x<105)
    	{
    		com.add(new ComandoGolpearBola(n,misJugadores[n].x+avanzarBolaX,misJugadores[n].y+avanzarBolaY));
    	}
    	else
    	{
            if (  this.getDistancia(misJugadores[n].x, misJugadores[n].y, 160, 0)<60   ||   n>8   ||   ( this.getDistancia(misJugadores[n].x, misJugadores[n].y, 160, 0)<90 && distanciaAl9<15 && distanciaAl10<15 && Math.abs(misJugadores[n].y)<65)    ||  ( ((n==1)||(n==2)||(n==3)) && this.getDistancia(misJugadores[n].x, misJugadores[n].y, 160, 0)<100)) 
            {            	
            	com.add(new ComandoTiroAPuerta(n,0));
                     	       	
            }
            else if(n==7 ||n==8)
            {
                	if (distanciaAl9>distanciaAl10)
                	{
                		
                		com.add(new ComandoGolpearBola(n,(misJugadores[9].x)+7,misJugadores[9].y));        		
                		
                	}
                	else
                	{
                		
                		com.add(new ComandoGolpearBola(n,(misJugadores[10].x)+7,misJugadores[10].y));        		
                		
                	}
            
            	

            }
            else if(n==0)
            {        
            	double maxDistancia=-1;
            	int jrp=-1;
            	
            	if (misJugadores[0].y >15)
            	{
            
                	if (distanciaAl5>maxDistancia)
                	{
                		maxDistancia=distanciaAl5;
                		jrp=5;
                	}
                	if (distanciaAl6>maxDistancia)
                	{
                		maxDistancia=distanciaAl6;
                		jrp=6;
                	}
                	if( maxDistancia<15 || this.getDistancia(bola.x, bola.y, misJugadores[jrp].x,misJugadores[jrp].y)>65)
                	{
                		if (distanciaAl2>15)
                		{
                			jrp=2;
                		}
                	}
            	}
            	else if (misJugadores[0].y<-15)
            	{
            		if (distanciaAl4>maxDistancia)
                	{
                		maxDistancia=distanciaAl4;
                		jrp=4;
                	}
                	if (distanciaAl5>maxDistancia)
                	{
                		maxDistancia=distanciaAl5;
                		jrp=5;
                	}
                	
                	if( maxDistancia<15 || this.getDistancia(bola.x, bola.y, misJugadores[jrp].x,misJugadores[jrp].y)>55)
                	{
                		if (distanciaAl3>15)
                		{
                			jrp=3;
                		}
                	}
            		
            	}
            	else
            	{
            		if (distanciaAl4>maxDistancia)
                	{
                		maxDistancia=distanciaAl4;
                		jrp=4;
                	}
                	if (distanciaAl5>maxDistancia)
                	{
                		maxDistancia=distanciaAl5;
                		jrp=5;
                	}
                	if (distanciaAl6>maxDistancia)
                	{
                		maxDistancia=distanciaAl6;
                		jrp=6;
                	}
                	if( maxDistancia<15 || this.getDistancia(bola.x, bola.y, misJugadores[jrp].x,misJugadores[jrp].y)>65)
                	{
                		if (distanciaAl1>15)
                		{
                			if (distanciaAl1>maxDistancia)
                        	{
                        		maxDistancia=distanciaAl1;
                        		jrp=1;
                        	}
                			
                		}
                		if (distanciaAl2>15)
                		{
                			if (distanciaAl2>maxDistancia)
                        	{
                        		maxDistancia=distanciaAl2;
                        		jrp=2;
                        	}
                			
                		}
                		if (distanciaAl3>15)
                		{
                			if (distanciaAl3>maxDistancia)
                        	{
                        		maxDistancia=distanciaAl3;
                        		jrp=3;
                        	}
                			
                		}
                	}
            	}
            	
   	
            	com.add(new ComandoGolpearBola(0,(misJugadores[jrp].x)+7,misJugadores[jrp].y));
            	
            	
            }
            else
            {
            	if  (distanciaAl9>10 || distanciaAl10>10)
            	{
            		double maxDistancia=-1;
                	int jrp=-1;
                	
                	if (distanciaAl9>maxDistancia)
                	{
                		maxDistancia=distanciaAl9;
                		jrp=9;
                	}
                	if (distanciaAl10>maxDistancia)
                	{
                		maxDistancia=distanciaAl10;
                		jrp=10;
                	}
                	com.add(new ComandoGolpearBola(n,(misJugadores[jrp].x)+7,misJugadores[jrp].y));
            	}
            	else
            	{
            		        	
                	double maxDistancia=-1;
                	int jrp=-1;
                	if ( (distanciaAl7-10)>maxDistancia && this.getDistancia(bola.x, bola.y, misJugadores[7].x, misJugadores[7].x)< 50)
                	{
                		maxDistancia=distanciaAl7-10;
                		jrp=7;
                	}
                	if ( (distanciaAl8)-10 >maxDistancia && this.getDistancia(bola.x, bola.y, misJugadores[8].x, misJugadores[8].x)< 50)
                	{
                		maxDistancia=distanciaAl8-10;
                		jrp=8;
                	}
                	if (distanciaAl9>maxDistancia)
                	{
                		maxDistancia=distanciaAl9;
                		jrp=9;
                	}
                	if (distanciaAl10>maxDistancia)
                	{
                		maxDistancia=distanciaAl10;
                		jrp=10;
                	}
                	com.add(new ComandoGolpearBola(n,(misJugadores[jrp].x)+7,misJugadores[jrp].y));
            	}
            }
        		     	
    	}
    	
    	
    return com;
    }
    
    
   
    
    private  Vector<Comando> mover(Vector<Comando> com, SituacionJugadores sj)
    {        
        double coenfAdX=sj.getBola().getX()/2;
        double coenfAdY=sj.getBola().getY()/3;
        
        
        double posicionesActuales[][]={
                {-150,0},  
                {this.control(-80+coenfAdX,-110,0),this.control(0+coenfAdY,-30,30)}, 
                {this.control(-70+coenfAdX,-100,0),this.control(45+coenfAdY,30,90)},  
                {this.control(-70+coenfAdX,-100,0),this.control(-45+coenfAdY,-90,-30)},  
                {this.control(-5+coenfAdX,-80,110),this.control(-30+coenfAdY,-90,-5)}, 
                {this.control(-20+coenfAdX,-90,10),this.control(0+coenfAdY,-20,20)}, 
                {this.control(-5+coenfAdX,-80,110),this.control(30+coenfAdY,5,90)},  
                {this.control(60+coenfAdX,0,140),this.control(50+coenfAdY,10,100)}, 
                {this.control(60+coenfAdX,0,140),this.control(-50+coenfAdY,-100,-10)}, 
                {this.control(60+coenfAdX,30,140),this.control(15+coenfAdY,0,30)},  
                {this.control(60+coenfAdX,30,140),this.control(-15+coenfAdY,-30,0)}  
            }; 
        
    
         
        for (int n=1 ; n<11 ; n++)
        { 
        	com.add(new ComandoIrA(n,posicionesActuales[n][0],posicionesActuales[n][1]));
        }
         
    return com;
    }
    
   
   
   private void deducirPuntoFinalBola()
   {
	   if (this.distRecAnt>11)
	   {
		   this.obtenerDistanciaRecorridaOptima();
	   }
	   else
	   {
		   this.distRecAnt=Math.rint(this.getDistancia (this.ba[0], this.ba[1],this.bac[0], this.bac[1])) ;
		  this.distRecAnt=this.control(distRecAnt, 0, 21);
		   
	   }
	   
	   double hipotenusa=0; 
	   for (double i=this.distRecAnt;i>0;i--)
	   {
		   hipotenusa+=i;
	   }
	   
	   this.bolasup[0]=Math.cos(this.anguloba)*hipotenusa + this.ba[0];
	   this.bolasup[1]=Math.sin(this.anguloba)*hipotenusa + this.ba[1];
	   

   }
    
    
    
   private void deducirJugadorMejorPosicionado(SituacionJugadores sj)
   {
	   if (this.distRecAnt>11)
	   {
		   this.obtenerDistanciaRecorridaOptima();
	   }
	   else
	   {
		  this.distRecAnt=Math.rint(this.getDistancia (this.ba[0], this.ba[1],this.bac[0], this.bac[1])) ;
		  this.distRecAnt=this.control(distRecAnt, 0, 21);		   
	   }
	   
	   double hipotenusa=0;
	   for (double i=this.distRecAnt;i>=15;i--)
	   {
		   hipotenusa+=i;
	   }
	   
	   this.xAccesible=Math.cos(this.anguloba)*hipotenusa + this.ba[0];
	   this.yAccesible=Math.sin(this.anguloba)*hipotenusa + this.ba[1];

	   
	   this.jugmio=sj.getMiJugadorMasCerca(xAccesible, yAccesible);
	   	   

   }
   
   private void obtenerDistanciaRecorridaOptima()
   {
	   double distanciaOptima=999;
	   double diferencia=999;
	   
	   if (Math.abs(this.distRecAnt-21) <diferencia)
	   {
		   diferencia=Math.abs(this.distRecAnt-21);
		   distanciaOptima=21;
	   }
	   if (Math.abs(this.distRecAnt-18) <diferencia)
	   {
		   diferencia=Math.abs(this.distRecAnt-18);
		   distanciaOptima=18;
	   }
	   if (Math.abs(this.distRecAnt-15) <diferencia)
	   {
		   diferencia=Math.abs(this.distRecAnt-15);
		   distanciaOptima=15;
	   }
	   if (Math.abs(this.distRecAnt-12) <diferencia)
	   {
		   diferencia=Math.abs(this.distRecAnt-12);
		   distanciaOptima=12;
	   }
	   
	   this.distRecAnt=distanciaOptima;
	   
   }
   
   
  
   private int buscarOtroJugadorPorDetras(SituacionJugadores sj)
   {
   	Point bola=sj.getBola();
   	Point misJugadores[]=sj.getMisJugadores();
   	
   	int jug=-1;
   	double distanciaABalon=99999999;
   	
   	for (int i=1;i<11;i++)
   	{
   		if (i!=sj.getMasCercanoDeBola() && misJugadores[i].x < bola.x )
   		{
   			if (this.getDistancia(misJugadores[i].x,misJugadores[i].y,bola.x,bola.y)<distanciaABalon)
   			{
   				distanciaABalon=this.getDistancia(misJugadores[i].x,misJugadores[i].y,bola.x,bola.y);
   				jug=i;
   			}
   		}
   			
   	}
   	
   	
   	
   	
   	
   	return jug;
   }
   
   private int buscarOtroJugador(SituacionJugadores sj)
   {
   	Point bola=sj.getBola();
   	Point misJugadores[]=sj.getMisJugadores();
   	
   	int jug=-1;
   	double distanciaABalon=99999999;
   	
   	for (int i=1;i<11;i++)
   	{
   		if (i!=sj.getMasCercanoDeBola())
   		{
   			if (this.getDistancia(misJugadores[i].x,misJugadores[i].y,bola.x,bola.y)<distanciaABalon)
   			{
   				distanciaABalon=this.getDistancia(misJugadores[i].x,misJugadores[i].y,bola.x,bola.y);
   				jug=i;
   			}
   		}
   			
   	}
   	
   	
   	
   	
   	
   	return jug;
   }
   


    
    
   private double getAngulo(double x0, double y0, double xD, double yD){
       double ang=0;
       double dy=yD-y0;
       double dx=xD-x0;
       if (dx>0) ang=Math.atan(dy/dx);
       else if (dx<0) ang=Math.atan(dy/dx)+Math.PI;
       else if (y0<yD) ang=Math.PI/2;
       else ang=-Math.PI/2;
       ang=corregirAngulo(ang);
       return ang;
   }
   
    private double corregirAngulo(double ang){
       while (ang<0)
           ang+=Math.PI*2;
       while (ang>=Math.PI*2)
           ang-=Math.PI*2;
       return ang;
   }
    
   private double getDistancia(double x1, double y1, double x2, double y2){
       return Math.sqrt(Math.pow(x1-x2,2)+Math.pow(y1-y2,2));
   }
   
   private double control(double numero, double min, double max){
       if (numero<min) return min;
       if (numero>max) return max;
       return numero;
   }
    
}

